function [ output, roiX, roiZ ] = tdSAFT2D( scanData, varargin )
%UNTITLED2 Summary of this function goes here
%   Detailed explanation goes here
%
scanDataValidationFcn = @(x) isstruct(x) && isfield(x, 'CscanData') && ...
    isfield(x, 'timeScale') && isfield(x, 'AscanValues');
validOutput = {'dB','normalized','raw'};
checkOutput = @(x) any(validatestring(x,validOutput));
validPostproc = {'env','rectified','none'};
checkPostproc = @(x) any(validatestring(x,validPostproc));

% Verify scanData parameter
%assert(scanDataValidationFcn(scanData), 'scanData invalid structure');

% Get scan data informations
Tresol = 1/60e6;%(scanData.timeScale(2) - scanData.timeScale(1))*60e-6;
Tini = scanData.timeScale(1)*Tresol;
Tmax = scanData.timeScale(end)*Tresol;
if strcmp(scanData.CscanData.WaveType, 'L')
    c = scanData.CscanData.Cl;
else
    c = scanData.CscanData.Cs;
end
ERMv = c/2;
iERMv = 2/c;
%maxVal = scanData.CscanData.MaxSignal;
maxVal= max(max(scanData.AscanValues))
scanX = scanData.CscanData.X;
scanY = scanData.CscanData.Y;
tamX = length(scanX);
tamY = length(scanY);
deltaX = scanX(2) - scanX(1);
minZ = 0;
maxZ = scanData.timeScale(end)*Tresol*ERMv;
deltaZ = Tresol*ERMv;

% Create parser to process arguments
inpP = inputParser;
% ROI X parameters
addParamValue(inpP, 'minX', scanX(1), @isnumeric);
addParamValue(inpP, 'maxX', scanX(end), @isnumeric);
% ROI Z parameters
addParamValue(inpP, 'minZ', minZ, @isnumeric);
addParamValue(inpP, 'maxZ', maxZ, @isnumeric);
% Plot output
addParamValue(inpP, 'plotOut', true, @islogical);
% Debug output
addParamValue(inpP, 'debug', false, @islogical);
% Y-slice selector
addParamValue(inpP, 'Yslice', find(scanY==0), @isnumeric);
% Output style
addParamValue(inpP, 'output', validOutput{1}, checkOutput); 
% dB limit
addParamValue(inpP, 'dBlim', -26, @isnumeric);
% X step
addParamValue(inpP, 'stepX', 1, @isnumeric);
% Post process
addParamValue(inpP, 'Postproc', validPostproc{1}, checkPostproc);
% Profile plot
addParamValue(inpP, 'profile', true, @islogical);

% Parse input arguments
parse(inpP, varargin{:});

% Define roiX
roiX = inpP.Results.minX:deltaX:inpP.Results.maxX
tamRoiX = length(roiX);

%roiZ = 3800/60e6:1/60e6:4200/60e6;
%Define roiZ
if(isfield(scanData.CscanData, 'DepthBW'))
    roiZ = inpP.Results.minZ:deltaZ:scanData.CscanData.DepthBW*1e-3;
else
    roiZ = inpP.Results.minZ:deltaZ:inpP.Results.maxZ;
end
tamRoiZ = length(roiZ)-1;

% Verify Y-slice
if(tamY > 1)
    assert(inpP.Results.Yslice > 0 && inpP.Results.Yslice <= tamY, ...
        'Yslice parameter out of range');
    yi = inpP.Results.Yslice;
else
    yi = 1;
end

% Get X step
stepX = inpP.Results.stepX;
stepMsg = sprintf('X step: %d', stepX);

output = zeros(tamRoiZ, tamRoiX);
if(inpP.Results.debug)
    tic;
end

% Line scan
ci = (yi - 1)*tamX;
xi = 1:tamX;
for xli = 1:tamRoiX
    xl = roiX(xli);
    dx2 = (scanX - xl).^2;
    for zli = 1:tamRoiZ
        zl2 = roiZ(zli)^2;
        R = sqrt(dx2 + zl2);
        tl = iERMv*R - Tini;
        tl = ((tl > 0) & (tl <= Tmax)).*tl;
        tli = round(tl./Tresol) + 1;
      
        v = diag(scanData.AscanValues(tli, ci + xi));
        s = sum(v(1:stepX:end));
        output(zli, xli) = (stepX*s/(tamX*maxVal));
    end
end

if(inpP.Results.debug)
    toc
end

% Output post-process
switch inpP.Results.Postproc
    case 'env'
        output = abs(hilbert(output));
        envMsg = 'td SAFT-2D envelope image';
        profMsg = 'td SAFT-2D envelope profile';
    case 'rectified'
        output = (output > 0).*output;
        envMsg = 'td SAFT-2D rectified image';
        profMsg = 'td SAFT-2D rectified profile';
    case 'none'
        envMsg = 'td SAFT-2D image';
        profMsg = 'td SAFT-2D profile';
end


switch inpP.Results.output
    case 'raw'
        titleProfMsg = {[profMsg ' (raw)'], ...
            sprintf('Y Slice: %d', inpP.Results.Yslice)};
        titleEnvMsg = {[envMsg ' (raw)'], ...
            sprintf('Y Slice: %d', inpP.Results.Yslice)};
        imgLimits = [min(min(output)) max(max(output))];
    case 'dB'
        output = abs(output);
        output = 20*log10(output ./ max(max(output)));
        titleProfMsg = {[profMsg sprintf(' (%.2f dB)', inpP.Results.dBlim)], ...
            sprintf('Y Slice: %d', inpP.Results.Yslice)};
        titleEnvMsg = {[envMsg sprintf(' (%.2f dB)', inpP.Results.dBlim)], ...
            sprintf('Y Slice: %d', inpP.Results.Yslice)};
        imgLimits = [inpP.Results.dBlim 0];
    case 'normalized'
        output = output ./ max(max(abs(output)));
        titleProfMsg = {[profMsg ' (normalized)'], ...
            sprintf('Y Slice: %d', inpP.Results.Yslice)};
        titleEnvMsg = {[envMsg ' (normalized)'], ...
            sprintf('Y Slice: %d', inpP.Results.Yslice)};
        imgLimits = [0 1];
end

% Plot results
if(inpP.Results.plotOut)
    if(inpP.Results.profile)
        figure; plot(roiX, max(output));
        titleProfMsg{length(titleProfMsg) + 1} = stepMsg;
        title(titleProfMsg);
        xlabel('X (m)');
        ylabel('max value');
        grid on;
    end    
    figure;
    subplot(211)
    imagesc(roiX*1000, roiZ*1000, output, imgLimits);
    colormap(jet);
    colorbar;
    titleEnvMsg{length(titleEnvMsg) + 1} = stepMsg;
    title(titleEnvMsg);
    xlabel('X (mm)');
    ylabel('Z - depth (mm)');
    grid on;
    subplot(212)
    imagesc(abs(scanData.AscanValues));
end
end
